
(*reading in the human data*)
(*LineNumber = 2;*)
IndexAllPre = 
  Import["/home/shu/workspace/Research/2DKnee_Torso_Outputs/data/\
IndexAll.mat", "MAT"];
IndexAll = Join[First[IndexAllPre]];
HipPosIndex = Round[IndexAll[[LineNumber, 1]]];
NSslopeIndex = Round[IndexAll[[LineNumber, 2]]];
TorsoIndex = Round[IndexAll[[LineNumber, 3]]];
(*choose for mean data or fred data*)
DataType = "mean";
(*choose the type of hip position output:
1. hip position;
2. linearized hip position;
3. Dimensionless linearized hip position*)

OutputTypeHippos = {"hippos", "LinearHippos", "LinearHipposD"};
hipposStr = 
  StringJoin[DataType, "_data_", OutputTypeHippos[[HipPosIndex]], 
   ".mat"];
(*choose the type of non-stance slope output:
1. Lineariezed non-stance slope;
2. non-stance slope;
3. Hip angle (angle between two thighs)*)

OutputTypeSlope = {"nsslope", "LinearNSslope", "hipAngle"};
nsslopeStr = 
  StringJoin[DataType, "_data_", OutputTypeSlope[[NSslopeIndex]], 
   ".mat"];
(*choose the type of torso angle output:
1. torso_hip angle;
2. theta4;
3. non-stance torso slope;
4. linearized torso slope;
5. stance torso slope;
6. linearized stance torso slope*)

OutputTypeTorso = {"TorsoHipAngle", "theta4", "nstorso", 
   "LinearNStorso", "storso", "LinearStorso","com","LinearCOM"};
TorsoStr = 
  StringJoin[DataType, "_data_", OutputTypeTorso[[TorsoIndex]], 
   ".mat"];
skneeStr = StringJoin[DataType, "_data_sknee", ".mat"];
nskneeStr = StringJoin[DataType, "_data_nsknee", ".mat"];

SetDirectory[
  "/home/shu/workspace/Research/2DKnee_Torso_Outputs/data/"];
NSslope = Import[nsslopeStr, "MAT"];
Sknee = Import[skneeStr, "MAT"];
NSknee = Import[nskneeStr , "MAT"];
Hippos = Import[hipposStr , "MAT"];
Torso = Import[TorsoStr, "MAT"];

A = Table[a[i, j], {i, 1, 5}, {j, 1, 5}];

Tf = Max[First[Torso]];
(*Data*)
HipData = Join[First[Hippos], Last[Hippos], 2];
(*NSslopeData = Join[First[NSslope],Last[NSslope],2];*)

NSslopeData = Join[First[NSslope], Last[NSslope], 2];
SkneeData = Join[First[Sknee], Last[Sknee], 2];
NSkneeData = Join[First[NSknee], Last[NSknee], 2];
TorsoData = Join[First[Torso], Last[Torso], 2];
Tf = Max[First[Torso]];

(*Parameters*)
HipParameters = {a[1, 1]};
NSslopeParameters = A[[2, ;;]];
SkneeParameters = A[[3, ;;]];
NSkneeParameters = A[[4, ;;]];
TorsoParameters = A[[5, ;;]];
Parameters = 
  Join[Join[
    Join[Join[HipParameters, NSslopeParameters], SkneeParameters], 
    NSkneeParameters], TorsoParameters];

(*Models*)
HipModel = Function[{t}, a[1, 1]*t];
NSslopeModel = 
  Function[{t}, (a[2, 1] Cos[a[2, 2] *t] + a[2, 3] Sin[a[2, 2] *t])/
    Exp[a[2, 4] *t] + a[2, 5]];
SkneeModel = 
  Function[{t}, (a[3, 1] Cos[a[3, 2] *t] + a[3, 3] Sin[a[3, 2] *t])/
    Exp[a[3, 4] *t] + a[3, 5]];
NSkneeModel = 
  Function[{t}, (a[4, 1] Cos[a[4, 2] *t] + a[4, 3] Sin[a[4, 2] *t])/
    Exp[a[4, 4] *t] + a[4, 5]];
TorsoModel = 
  Function[{t}, (a[5, 1] Cos[a[5, 2] *t] + a[5, 3] Sin[a[5, 2] *t])/
    Exp[a[5, 4] *t] + a[5, 5]];

(*TorsoModel =Function[{t},(a[5,1] Cos[a[5,2] *t]+a[5,3] Sin[a[5,2] \
*t])/Exp[a[5,4] *t]+a[5,5]];*)
TorsoModel2 = Function[{t}, a[5, 5]];
(*Fits*)

HipFit = FindFit[HipData, HipModel[t], HipParameters, t];

NSslopeFit = 
  FindFit[NSslopeData, NSslopeModel[t], NSslopeParameters, t];

SkneeFit = FindFit[SkneeData, SkneeModel[t], SkneeParameters, t];

NSkneeFit = FindFit[NSkneeData, NSkneeModel[t], NSkneeParameters, t];

TorsoFit = 
  FindFit[TorsoData, 
   TorsoModel[t], {a[5, 1], {a[5, 2], 10}, a[5, 3], a[5, 4], a[5, 5]},
    t];

(*HumanParameters*)

HumanParameters = 
  Parameters /. HipFit /. NSslopeFit /. SkneeFit /. NSkneeFit /. 
   TorsoFit;
HumanFit = 
  Join[Join[Join[Join[HipFit, NSslopeFit], SkneeFit], NSkneeFit], 
   TorsoFit];

(*Plots*)

(*Show[ListPlot[HipData],Plot[HipModel[t]/.HumanFit,{t,0,Tf}]]

Show[ListPlot[NSslopeData],Plot[NSslopeModel[t]/.HumanFit,{t,0,Tf}]]

Show[ListPlot[SkneeData],Plot[SkneeModel[t]/.HumanFit,{t,0,Tf}]]

Show[ListPlot[NSkneeData],Plot[NSkneeModel[t]/.HumanFit,{t,0,Tf}]]

Show[ListPlot[TorsoData],Plot[TorsoModel[t]/.TorsoFit,{t,0,Tf}]]*)

matrixName = "Afit" <> ToString[LineNumber];
Aout = A /. HumanFit /. {a[1, 2] -> 0, a[1, 3] -> 0, a[1, 4] -> 0, 
    a[1, 5] -> 0, a[2, 5] -> 0, a[5, 3] -> 0, a[5, 4] -> 0, 
    a[5, 5] -> 0};

SetDirectory[
  "/home/shu/workspace/Research/2DKnee_Torso_Outputs/data/"];
Export[matrixName <> ".mat", "a0" -> Aout, "LabeledData"];
